/*
 * sccilp.c, SmartCam coprocessor library for XeTaL (1)
 *
 */

#define _USE_MATH_DEFINES

#include <string.h>
#include <errno.h>
#include <math.h>
#include <log.h>
#include <getopt.h>
#include <scos.h>
#include <Rhapsody.h>
#include <RapError.h>
 
#include "XetalGlobals.h"
#include "Xetal.h"

#include "sc.h"
#include "scc.h"
#include "scu.h"
#include "scxtl.h"

log_default_streams_t xtll;
static table_t xtl_ops, xtl_bufs, xtl_opids, xtl_linemems;
static int xtl_proc, xtl_dest_proc, xtl_dest_buf;
static scos_mutex_t xtl_dest_lock, xtl_lock;
static scos_cond_t xtl_dest_condition;
static int capture_ready = 0;
tmr_t frameTime;

IMAGE imageDisplay, imageOverlay, imageCapture;
unsigned char *disptr, *captr;

void scXtlInit(int proc)
{
  static int first=1;
  int id, i;
  scos_thread_t thread;

  abortOnError(STREAM(xtll.err), !first);

  logAddGroup();
  xtll = logAddDefaultStreams(NULL);
  logEdit(xtll.crawl, LOG_GROUP_SET_LEVEL);

  lprintf(STREAM(xtll.notice), "Initializing SmartCam XeTaL 1 coprocessor %d", proc);
  xtl_proc = proc;

  tblcreate(&xtl_ops, sizeof(xtl_op_t));
  tblcreate(&xtl_bufs, sizeof(int));
  tblcreate(&xtl_opids, sizeof(int));
  tblcreate(&xtl_linemems, sizeof(int));

  sccCoprocessorSetup(proc, xtlProcessMessage, xtlTransportReceiver);

  scos_mutex_init(&xtl_lock);
  scos_mutex_init(&xtl_dest_lock);
  scos_cond_init(&xtl_dest_condition);

  frameTime = tmrInit();

  /* Initialise Rhapsody */
  xtlRhapsodyInit();

  /* Spawn capture thread */
  scos_thread_create(&thread, xtlCapture, NULL);

  while (!capture_ready) sched_yield();
}

void xtlRhapsodyInit(void)
{
  RAP_CALL( XetalInactivateI2C() );
  
  // configure and enable the display and overlay
  RAP_CALL
  ( 
	  RapDisplayMode( RAP_DISPLAY_640x480x60HZ,
                      RAP_OVERLAY_100_PERCENT,
					  SENSOR_HEIGHT,
					  SENSOR_WIDTH,
                      &imageDisplay,
                      &imageOverlay)
  );

  RAP_CALL ( RapImgGetPar(imageDisplay, RAP_PAR_IMAGE_POINTER, (long*)&disptr) );

  RAP_CALL( RapDisplayEnable( RAP_ON, RAP_ON) );

  // Define the Pixelcam sensor (=default for Inca).
  RAP_CALL( RapCameraDefine (RAP_PCS_2112_8_BITS, RAP_CONTINUOUS_MODE, RAP_NONE ) );
  
  // Set the integration time.
  RAP_CALL( RapCameraSetPar( RAP_PAR_INTEGRATION_SIZE, NR_INTEGRATION_LINES) );
  
  // Set Gain
  RAP_CALL( RapCameraSetPar( RAP_PAR_GAIN_CHANNEL_A, 0) );
  
  // Set the sensor in normal capture mode (=default);                    
  RAP_CALL( RapSetTimeout( RAP_TIMEOUT_CAPTURE, 1000) );

  RAP_CALL( RapCameraSetPar( RAP_PAR_OUTPUT_MODE, RAP_MODE_NORMAL /* RAP_MODE_TEST */ ) );

  RAP_CALL( RapCameraSetPar(	RAP_PAR_HORIZONTAL_DIRECTION, RAP_DIRECTION_MIRRORED ) );

  // Set pixelclock sensor
  RAP_CALL( RapCameraSetPar (RAP_PAR_FREQUENCY, 16) );

  // Xetal stuff                                
  RAP_CALL( XetalActivateI2C() );
  RAP_CALL( XetalLoadProgram("/flash/sctest.cmd") );
  RAP_CALL( XetalWriteParam() );
  RAP_CALL( XetalOn() );

  // Setup images for continuous capturing
  RAP_CALL ( RapImgAlloc(0, SENSOR_HEIGHT, SENSOR_WIDTH, RAP_PIXEL_8_BITS, &imageCapture) );

  /* Get image pointer */
  RAP_CALL ( RapImgGetPar(imageCapture, RAP_PAR_IMAGE_POINTER, (long*)&captr) );

  xtlStartCapture();

#if 0
  while (1)
  {
    RAP_CALL
    (
      RapCaptureExtended( RAP_CHANNEL_A,          /* Must be channel A */
                        RAP_CAPTURE_FRAME,
                        0, 
                        RAP_CAPTURE_NOWAIT,
  					    SENSOR_STARTROW,
						SENSOR_STARTCOL,
						SENSOR_HEIGHT,
						SENSOR_WIDTH,
	                    SUBSAMPLE_FACTOR,
	                    SUBSAMPLE_FACTOR,
                        NULL,
                        imageDisplay)

    );
    RAP_CALL( RapCaptureWait(RAP_CHANNEL_A) );
  }
#endif

#if 0
  RapCaptureExtended( RAP_CHANNEL_A,
                      RAP_CAPTURE_FRAME|RAP_CAPTURE_CONTINUOUS,
                      0, 
                      RAP_CAPTURE_NOWAIT,
  					  SENSOR_STARTROW,
					  SENSOR_STARTCOL,
					  SENSOR_HEIGHT,
					  SENSOR_WIDTH,
	                  SUBSAMPLE_FACTOR,
	                  SUBSAMPLE_FACTOR,
                      NULL,
                      imageDisplay);
#endif

#if 0
  xtlWriteCommand(0, 0, 0, 0);
  xtlWriteCommand(1, 1, 0, 0);

  while (1)
  {
    RapCaptureExtended( RAP_CHANNEL_A,
                      RAP_CAPTURE_FRAME,
                      0, 
                      RAP_CAPTURE_NOWAIT,
  					  SENSOR_STARTROW,
					  SENSOR_STARTCOL,
					  SENSOR_HEIGHT,
					  SENSOR_WIDTH,
	                  SUBSAMPLE_FACTOR,
	                  SUBSAMPLE_FACTOR,
                      NULL,
                      imageDisplay);

    RapCaptureWait( RAP_CHANNEL_A );

    xtlAwaitFinish();
  }
#endif

}

void xtlProcessMessage(scc_hdr_t *hdr)
{
  scos_thread_t thread;
  xtl_op_t *op;
  scc_msg_t *msg = (scc_msg_t*)hdr;
  int *l, args[3];
  int id;

  /* Make sure we're not in xtlCapture's critical section */
  switch (msg->header.type)
  {
    case SCC_MSGTYPE_BUFFERCREATE:
      abortOnNegative(STREAM(xtll.err), tbladdnewi(&xtl_bufs, &l, msg->data[0]));
      abortOnNegative(STREAM(xtll.err), *l = tbladdnew(&xtl_linemems, NULL));
      lprintf(STREAM(xtll.debug), "Created buffer %d at linemem %d", msg->data[0], *l);
      break;
    case SCC_MSGTYPE_BUFFERFINALIZE:
      /* Only 1 reader allowed */
      break;
    case SCC_MSGTYPE_TRANSPORTCREATE:
      lprintf(STREAM(xtll.debug), "Transmitting buffer %d to %d.%d",
	      *((int*)msg->data), msg->processor, *((int*)&msg->data[sizeof(int)]));

/*
      lprintf(STREAM(xtll.debug), "Data: %d %d %d %d %d %d %d %d",
          msg->data[0], msg->data[1], msg->data[2], msg->data[3], 
          msg->data[4], msg->data[5], msg->data[6], msg->data[7]);
*/

      abortOnNULL(STREAM(xtll.err), tblget(&xtl_bufs, *((int*)msg->data), &l));

      /* Setup Xetal to write buffer to output */
      args[0] = *l;
      args[1] = args[2] = 0;
      xtlWriteCommand(tbladdnew(&xtl_opids, NULL), TRANSPORT, args);

      /* Setup TriMedia to capture output to correct buffer */
      scos_mutex_lock(&xtl_dest_lock);
      xtl_dest_proc = msg->processor;
      xtl_dest_buf = *((int*)&msg->data[sizeof(int)]);
      scos_cond_signal(&xtl_dest_condition);
      scos_mutex_unlock(&xtl_dest_lock);
      break;
    case SCC_MSGTYPE_OPERATIONENQUEUE:
      lprintf(STREAM(xtll.debug),
	      "Enqueueing operation %d type %d (needs %d args)",
	      msg->opid, msg->optype, msg->size);
      abortOnNegative(STREAM(xtll.err), tbladdnewi(&xtl_ops, &op, msg->opid));
      op->id = msg->opid;
      op->xtlid = tbladdnew(&xtl_opids, NULL);
      op->operation = msg->optype;
      op->args[0] = op->args[1] = op->args[2] = 0;
      op->arguments = msg->size;      
      op->argreceived = 0;
      break;
    case SCC_MSGTYPE_OPERATIONARGUMENT:
      abortOnNULL(STREAM(xtll.err), tblget(&xtl_ops, msg->opid, &op));

      if (msg->argtype == SC_VAR_STREAM)
      {
        abortOnNULL(STREAM(xtll.err), tblget(&xtl_bufs, msg->data[0], &l));
        op->args[msg->argid] = *l;
      }
      else
        lprintf(STREAM(xtll.debug), "Discarding non-stream argument %d", msg->argid);

      op->argreceived++;

      if (op->argreceived == op->arguments)
      {
        lprintf(STREAM(xtll.info), "Starting operation %d", op->id);
        xtlWriteCommand(op->xtlid, op->operation, op->args);
      }
  }

}

void xtlWriteCommand(int opid, int optype, int *args)
{
  unsigned char read[3], got_opid;

  scos_mutex_lock(&xtl_lock);

  if (args)
  {
    lprintf(STREAM(xtll.info), "Writing xtl id %d type %d args {%d, %d, %d}", opid, optype, args[0], args[1], args[2]);
    d_i2c[ 7] = (args[0] | (args[1]<<3) | (args[2]<<6))>>8;  //  Exposure Time MSB
    d_i2c[ 8] = (args[0] | (args[1]<<3) | (args[2]<<6))&255; // Exposure Time LSB
  }
  else
  {
    lprintf(STREAM(xtll.info), "Writing xtl command %d", opid);
    d_i2c[ 7] = 0;      //  Exposure Time MSB
    d_i2c[ 8] = 0; // Exposure Time LSB
  }

  /* Pixel clock must be on for XeTaL to accept commands */
  if (xtlIsCaptureFinished()) xtlStartCapture();

  /* Only change registers which matter */
  d_i2c[ 9] = 0;      // Black reference MSB
  d_i2c[10] = optype; // Black reference LSB
  d_i2c[11] = 0;      // GAIN MSB
  d_i2c[12] = opid;   // GAIN LSB

  TrmI2cWriteBlock( 0x18, 13, d_i2c );
  
  read[0] = 0x07; /* i2c_register mux */
  read[1] = 0x01; /* get i2c parameters */
  read[2] = 0x02; /* lower byte of PREG2 */

  do
  {
    TrmI2cWriteBlock( 0x18, 3, read);
    TrmI2cRead(0x18, 1, &got_opid);
    if (got_opid != 31) sched_yield();
  } while (got_opid != 31);
  lprintf(STREAM(xtll.info), "Operation accepted by XeTaL");

  scos_mutex_unlock(&xtl_lock);
}

void xtlAwaitFinish(void)
{
  unsigned char read[3], got_finished;

  scos_mutex_lock(&xtl_lock);

  read[0] = 0x07; /* i2c_register mux */
  read[1] = 0x01; /* get i2c parameters */
  read[2] = 0x03; /* lower byte of PREG3 */

  /* Wait for finished register to be set */
  do
  {
    TrmI2cWriteBlock( 0x18, 3, read);
    TrmI2cRead(0x18, 1, &got_finished);
  } while (!got_finished);

  /* Only change registers which matter */
  d_i2c[11] = 0;      // GAIN MSB
  d_i2c[12] = 31;   // GAIN LSB
  d_i2c[13] = 0;      // start row MSB
  d_i2c[14] = 0;   // start row LSB

  /* Reset finished register */
  TrmI2cWriteBlock( 0x18, 15, d_i2c );

  lprintf(STREAM(xtll.info), "Frame finished");

  scos_mutex_unlock(&xtl_lock);
}

void xtlWriteResult(xtl_op_t *o, int id, int type, char *data)
{
  scc_msg_t msg;
 
  msg.header.source = xtl_proc;
  msg.header.destination = SCC_MASTER_PROCESSOR;
  msg.header.size = sizeof(scc_msg_t)-sizeof(scc_hdr_t);
  msg.header.type = SCC_MSGTYPE_OPERATIONRESULT;

  msg.opid = o->id;
  msg.optype = o->operation;
  msg.argid = id;
  msg.argtype = type;
  
  switch (msg.argtype)
  {
    case SC_VAR_INTEGER:
      lprintf(STREAM(xtll.debug),
	      "Sending operation %d VAR_INTEGER argument %d with value %d",
	      msg.opid, msg.argid, *((int*)data));
      memcpy(msg.data, data, sizeof(int));
      break;
    case SC_VAR_DOUBLE:
      lprintf(STREAM(xtll.debug),
	      "Sending operation %d VAR_DOUBLE argument %d with value %f",
	      msg.opid, msg.argid, *((double*)data));
      memcpy(msg.data, data, sizeof(double));
      break;
  }

  sccSendMessage((scc_hdr_t*)&msg);
}

void xtlOperationComplete(int id)
{
  scc_msg_t msg;

  msg.header.source = xtl_proc;
  msg.header.destination = SCC_MASTER_PROCESSOR;
  msg.header.size = sizeof(scc_msg_t)-sizeof(scc_hdr_t);
  msg.header.type = SCC_MSGTYPE_OPERATIONCOMPLETED;

  msg.opid = id;
  
  sccSendMessage((scc_hdr_t*)&msg);
}

void xtlBufferDestructor(int id)
{
  scc_msg_t msg;

  msg.header.source = xtl_proc;
  msg.header.destination = SCC_MASTER_PROCESSOR;
  msg.header.size = sizeof(scc_msg_t)-sizeof(scc_hdr_t);
  msg.header.type = SCC_MSGTYPE_BUFFERCOMPLETED;

  msg.argid = id;
  msg.argtype = SC_VAR_STREAM;
  *((int*)msg.data[0]) = 0; /* bytes */
  *((int*)msg.data[4]) = 0; /* counter */

  sccSendMessage((scc_hdr_t*)&msg);
}

void xtlTransportReceiver(scc_hdr_t *hdr)
{
}

void *xtlCapture(void *dummy)
{
  unsigned long int y, x;
  char buf[SCOS_MTU+sizeof(scc_datamsg_t)];
  scc_datamsg_t *msg = (scc_datamsg_t*) buf;
  int id=-1;
  xtl_op_t *op;
  table_t cops, cbufs;

  scos_mutex_lock(&xtl_dest_lock);

  xtlDrawOverlay();

  while (1)
  {
    capture_ready = 1;
    scos_cond_wait(&xtl_dest_condition, &xtl_dest_lock);

    lprintf(STREAM(xtll.debug), "Capturing a frame");

    /* Start a new capture */
    if (!xtlIsCaptureFinished())
    {
      while (!xtlIsCaptureFinished()) sched_yield();
    }

    xtlStartCapture();

    // await entire image
    if (!xtlIsCaptureFinished())
    {
      while (!xtlIsCaptureFinished()) sched_yield();
    }

    lprintf(STREAM(xtll.debug), "Transporting frame to %d.%d", xtl_dest_proc, xtl_dest_buf);

    msg->header.source = xtl_proc;
    msg->header.destination = xtl_dest_proc;
    msg->header.type = SCC_MSGTYPE_BUFFERDATA;
    msg->header.size = sizeof(scc_datamsg_t)-sizeof(scc_hdr_t)+SENSOR_WIDTH/SUBSAMPLE_FACTOR;  
    msg->buffer = xtl_dest_buf;

    tmrStart(&frameTime);

    /* Transport image */
    for (y=0; y < 480 /*SENSOR_HEIGHT/SUBSAMPLE_FACTOR*/; y++)
    {
      /* Wait for canary to drop */
      if (captr[y*SENSOR_WIDTH+SENSOR_WIDTH/SUBSAMPLE_FACTOR-2] == 's' &&
          captr[y*SENSOR_WIDTH+SENSOR_WIDTH/SUBSAMPLE_FACTOR-1] == 'C')
      {
        while (captr[y*SENSOR_WIDTH+SENSOR_WIDTH/SUBSAMPLE_FACTOR-2] == 's' &&
               captr[y*SENSOR_WIDTH+SENSOR_WIDTH/SUBSAMPLE_FACTOR-1] == 'C') sched_yield();
      }

      scos_trace(255);

      /* *** WARNING: DOESN'T RESPECT WINDOW *** */
      memcpy(&msg->data, &captr[y*SENSOR_WIDTH], SENSOR_WIDTH/SUBSAMPLE_FACTOR);
      sccSendMessage((scc_hdr_t*)msg);
    }

    //tmrStop(&frameTime);

    lprintf(STREAM(xtll.crawl), "Transport done. Cleaning up");

    /* Signal transport completion */
    msg->header.type = SCC_MSGTYPE_BUFFERDRY;
    msg->header.size = sizeof(scc_datamsg_t)-sizeof(scc_hdr_t);
    sccSendMessage((scc_hdr_t*)msg);

    /* Queue completion of all operations */
    tblcreate(&cops, sizeof(int));
    while (tblget(&xtl_ops, id = tblgetnexti(&xtl_ops, id), &op))
      abortOnNegative(STREAM(scl.err), tbladdnewi(&cops, NULL, id));

    tblclear(&xtl_ops, 1);
    tbldestroy(&xtl_opids, 1);
    tblcreate(&xtl_opids, sizeof(int));

    /* Queue destruction of all buffers */
    tblcreate(&cbufs, sizeof(int));
    while (tblget(&xtl_bufs, id = tblgetnexti(&xtl_bufs, id), NULL))
      abortOnNegative(STREAM(scl.err), tbladdnewi(&cbufs, NULL, id));

    tblclear(&xtl_bufs, 1);
    tbldestroy(&xtl_linemems, 1);
    tblcreate(&xtl_linemems, sizeof(int));

    /* Signal completion of all operations */
    while (tblget(&cops, id = tblgetnexti(&cops, id), NULL))
      xtlOperationComplete(id);
    tbldestroy(&cops, 1);

    /* Signal completion of all buffers */
    while (tblget(&cbufs, id = tblgetnexti(&cbufs, id), NULL))
      xtlBufferDestructor(id);
    tbldestroy(&cbufs, 1);

    lprintf(STREAM(xtll.crawl), "All spotless");

    /* Kill all XeTaL tasks */
    //xtlWriteCommand(30, 0, NULL);
  }
}

int xtlIsCaptureFinished(void)
{
  /* Check canary */
  if (captr[479*1280+638] != 's' || captr[479*1280+639] != 'C')
  {
    lprintf(STREAM(xtll.crawl), "Calling RapCaptureWait()");
    RapCaptureWait(RAP_CHANNEL_A);
    lprintf(STREAM(xtll.crawl), "RapCaptureWait() returned");
    return 1;
  }
  else
    return 0;
}

void xtlStartCapture(void)
{
  int y;

  /* Set canaries */
  for (y=0; y < SENSOR_HEIGHT/SUBSAMPLE_FACTOR; y++)
  {
    captr[y*SENSOR_WIDTH+SENSOR_WIDTH/SUBSAMPLE_FACTOR-2] = 's';
    captr[y*SENSOR_WIDTH+SENSOR_WIDTH/SUBSAMPLE_FACTOR-1] = 'C';
  }

  /* Run capture */
  abortOnError(STREAM(xtll.err), 
               RapCaptureExtended( RAP_CHANNEL_A,
               RAP_CAPTURE_FRAME,
               0, 
               RAP_CAPTURE_NOWAIT,
  			   SENSOR_STARTROW,
			   SENSOR_STARTCOL,
			   SENSOR_HEIGHT,
			   SENSOR_WIDTH,
	           SUBSAMPLE_FACTOR,
	           SUBSAMPLE_FACTOR,
               NULL,
               imageCapture) != RAP_ERROR_NONE);
}

#define IMAGE_MAIN_TOP              40
#define IMAGE_MAIN_BOTTOM           32
#define COLOUR_BAR_MAIN             0x000080
#define COLOUR_BAR_PANEL            0x0000A0
#define COLOUR_TEXT_MAIN            0xFFFFFF
#define COLOUR_TEXT_PANEL           0xFFFFFF

void xtlDrawOverlay()
{
  int iDummy;

  RAP_CALL ( RapDrawRectangle(
                            0,
                            0, 0,
                            IMAGE_MAIN_TOP, 640,
                            0.0,
                            COPY,
                            COLOUR_BAR_MAIN,
                            COLOUR_BAR_MAIN,
                            imageOverlay)
  );

  RAP_CALL ( RapDrawRectangle(
                            0,
                            480 - IMAGE_MAIN_BOTTOM, 0,
                            IMAGE_MAIN_BOTTOM, 640,
                            0.0,
                            COPY,
                            COLOUR_BAR_MAIN,
                            COLOUR_BAR_MAIN,
                            imageOverlay)
  );

  RAP_CALL ( RapDrawString(
                    0,
                    480 - IMAGE_MAIN_BOTTOM + 7, 10,
                    "Delft University of Technology, 2004",
                    RAP_FONT_ARIAL26,
                    0.0,
                    COPY,
                    COLOUR_TEXT_MAIN,
                    COLOUR_BAR_MAIN,
                    &iDummy,
                    imageOverlay)
  );

}

void xtlDrawFrameTime(double frametime, char *text)
{
  char buf[16];
  int iDummy;

  sprintf(buf, "%3.0f ms ", frametime/1000.0);

  RAP_CALL ( RapDrawString(
                    0,
                    0 + 7, 180,
                    text,
                    RAP_FONT_ARIAL26,
                    0.0,
                    COPY,
                    COLOUR_TEXT_MAIN,
                    COLOUR_BAR_MAIN,
                    &iDummy,
                    imageOverlay)
  );

  RAP_CALL ( RapDrawString(
                    0,
                    480 - IMAGE_MAIN_BOTTOM + 7, 530,
                    buf,
                    RAP_FONT_ARIAL26,
                    0.0,
                    COPY,
                    COLOUR_TEXT_MAIN,
                    COLOUR_BAR_MAIN,
                    &iDummy,
                    imageOverlay)
  );
}

void xtlDrawFrameTime2(double frametime)
{
  char buf[16];
  int iDummy;

  sprintf(buf, "%3.0f ms ", frametime/1000.0);

  RAP_CALL ( RapDrawString(
                    0,
                    480 - IMAGE_MAIN_BOTTOM + 7, 400,
                    buf,
                    RAP_FONT_ARIAL26,
                    0.0,
                    COPY,
                    COLOUR_TEXT_MAIN,
                    COLOUR_BAR_MAIN,
                    &iDummy,
                    imageOverlay)
  );
}

